/*****************************************************************
*  Copyright (C) 2012  Marco Bavagnoli - lil.deimos@gmail.com    *
******************************************************************/

#include "horizon.h"

#include <math.h>

Horizon::Horizon(QDeclarativeItem *parent) :
    QDeclarativeItem(parent),
    m_color(Qt::black),
    m_x1(0),
    m_y1(0),
    m_x2(0),
    m_y2(0),
    m_penWidth(1),
    m_active(false),
    m_rotation(0),
    m_reading(0),
    m_calibrateMinX(0),
    m_calibrateMaxX(9.8),
    m_calibrateMinY(0),
    m_calibrateMaxY(9.8),
    m_calibrateMinZ(0),
    m_calibrateMaxZ(9.8)
{
    setFlag(QGraphicsItem::ItemHasNoContents, true);
    setFlag(QGraphicsItem::ItemIgnoresParentOpacity, true);
    setAcceptedMouseButtons(0);
    setAcceptTouchEvents(false);

    m_rotation = new QAccelerometer();

    // Build circular list of samples
    for (int i = 0; i < ROT_SAMPLES; i++) {
        rotX_samples[i] = new listRot;
        rotY_samples[i] = new listRot;
        rotZ_samples[i] = new listRot;
    }
    for (int i = 0; i < ROT_SAMPLES; i++) {
        rotX_samples[i]->prev = (i>0 ? rotX_samples[i-1] : rotX_samples[ROT_SAMPLES]);
        rotX_samples[i]->next = (i<ROT_SAMPLES-1 ? rotX_samples[i+1] : rotX_samples[0]);
        rotX_samples[i]->value = i;

        rotY_samples[i]->prev = (i>0 ? rotY_samples[i-1] : rotY_samples[ROT_SAMPLES]);
        rotY_samples[i]->next = (i<ROT_SAMPLES-1 ? rotY_samples[i+1] : rotY_samples[0]);
        rotY_samples[i]->value = i;

        rotZ_samples[i]->prev = (i>0 ? rotZ_samples[i-1] : rotZ_samples[ROT_SAMPLES]);
        rotZ_samples[i]->next = (i<ROT_SAMPLES-1 ? rotZ_samples[i+1] : rotZ_samples[0]);
        rotZ_samples[i]->value = i;
    }
    currRotX = rotX_samples[0];
    currRotY = rotY_samples[0];
    currRotZ = rotZ_samples[0];

    connect(m_rotation, SIGNAL(activeChanged()), this, SLOT(onItemActiveChanged()));
    connect(m_rotation, SIGNAL(readingChanged()), this, SLOT(onReadingChanged()));
}
Horizon::~Horizon()
{
    m_rotation->stop();
    delete m_rotation;
    m_rotation=0;
    m_reading=0;
}

bool Horizon::findIntersection(qreal x11, qreal y11, qreal x12, qreal y12,
                               qreal x21, qreal y21, qreal x22, qreal y22,
                               qreal &x , qreal &y )
{
    qreal m1 = (y12-y11)/(x12-x11);
    qreal m2 = (y22-y21)/(x22-x21);
    qreal b1 = y11 - m1*x11;
    qreal b2 = y21 - m2*x21;
    x=y=0;
    if (m1 == m2) return false;
    if (m1 == m2 && b1 == b2) return false;

    y = m1*((b2-b1)/(m1-m2))+b1;
    x = (b2-b1)/(m1-m2);
    return true;
}

void Horizon::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
    Q_UNUSED(option)
    Q_UNUSED(widget)
    qreal rotX, rotY, rotZ;

    // If the device is horizontal +- 45� then exit
    if ( qAbs(m_average_rotZ) > (m_calibrateMaxZ-m_calibrateMinZ)/2 ) return;

    painter->setPen(pen);

    if(smooth() == true)
        painter->setRenderHint(QPainter::Antialiasing, true);

    // Normalize rotation values to PI/2
    rotX = (m_average_rotX-m_calibrateMinX)*M_PI_2/(m_calibrateMaxX-m_calibrateMinX);
    rotY = (m_average_rotY-m_calibrateMinY)*M_PI_2/(m_calibrateMaxY-m_calibrateMinY);
    rotZ = (m_average_rotZ-m_calibrateMinZ)*M_PI_2/(m_calibrateMaxZ-m_calibrateMinZ);
    qreal teta;
    teta = -atan(rotY/rotX);


    // Calculate different average position for portrait and landscape
    qreal w,h;
    if ( qAbs(m_average_rotY) < (m_calibrateMaxY-m_calibrateMinY)/2 ) {
        w = boundingRect().width();
        h = boundingRect().height();
        if ( m_average_rotX<0 ) h *= -1;
    }
    else {
        w = boundingRect().height();
        h = boundingRect().width();
        if ( m_average_rotY<0 ) { w *= -1; h *= -1; }
    }
    // Calculate 2 point of the horizon slope
    // At 45� deg the horizon should be at top of the screen (rotZ*2)
    m_x1 = boundingRect().width()/2  + cos( (rotZ*2+M_PI_2) )*w/2;
    m_y1 = boundingRect().height()/2 + cos( (rotZ*2+M_PI_2) )*h/2;

    m_x2 = m_x1 + cos(teta)*200;
    m_y2 = m_y1 + sin(teta)*200;
//painter->drawLine(m_x1, m_y1,   m_x2, m_y2);

    // With the calculated slope ( (y2-y1)/(x2-x1) ) get the intesction with the (2 edges) bounding rectangle
    qreal x,y;
    qreal x1,y1,x2,y2;
    qreal *px, *py;

    px=&x1;
    py=&y1;

    // Find intersection with upper edge. When a pair is found px and py will point to the x2 and y2 values to set them
    if ( findIntersection( m_x1,m_y1, m_x2,m_y2,  0,0, 1,0,  x,y ) &&
         ( x <= boundingRect().width() && x>=0 ) ) {
        *px=x;
        *py=0;
        px=&x2;
        py=&y2;
    }

    // Find intersection with bottom edge
    if ( findIntersection( m_x1,m_y1, m_x2,m_y2,  0,boundingRect().height(), 1,boundingRect().height(),  x,y ) &&
         ( x <= boundingRect().width() && x>=0 ) ) {
        *px=x;
        *py=boundingRect().height();
        px=&x2;
        py=&y2;
    }

    // Find intersection with left edge (add a little bit to x2 to avoid nan)
    if ( findIntersection( m_x1,m_y1, m_x2,m_y2,  0,0, 0.001,10,  x,y ) &&
         ( y <= boundingRect().height() && y>=0 ) ) {
        *px=0;
        *py=y;
        px=&x2;
        py=&y2;
    }

    // Find intersection with right edge (add a little bit to x2 to avoid nan)
    if ( findIntersection( m_x1,m_y1, m_x2,m_y2,  boundingRect().width(),0, boundingRect().width()+0.001,10,  x,y ) &&
         ( y <= boundingRect().height() && y>=0) ) {
        *px=boundingRect().width();
        *py=y;
    }

    painter->drawLine(x1, y1,   x2, y2);
}

///////////////////////////////////////////////////////////
// Set methods
void Horizon::setColor(QColor &color) {
    if(m_color == color) return;
    m_color = color;
    pen.setColor(m_color);
    update();
    emit colorChanged();
}

void Horizon::setPenWidth(int newWidth) {
    if(m_penWidth == newWidth) return;
    m_penWidth = newWidth;
    pen.setWidth(m_penWidth);
    update();
    emit penWidthChanged();
}

void Horizon::setActive(bool b)
{
    m_active = b;
    m_rotation->setActive(b);
    if (b) {
        m_rotation->start();
        m_reading = m_rotation->reading();
    }
    else m_rotation->stop();
}

///////////////////////////////////////////////////////////
// Slots
void Horizon::onItemActiveChanged()
{
    if (m_active)
        setFlag(QGraphicsItem::ItemHasNoContents, false);
    else
        setFlag(QGraphicsItem::ItemHasNoContents, true);
}

// Slot called when the circular list isn't filled yet. When it is filled, change the connected slot to onReadingChanged2()
void Horizon::onReadingChanged()
{
    // fill the circular list of samples
    if (!m_reading) return;
    if (!m_rotation->isActive() || m_rotation->isBusy()) return;
    static int count = 0;

    rotX_samples[count]->value = m_reading->x();
    rotY_samples[count]->value = m_reading->y();
    rotZ_samples[count]->value = m_reading->z();

    // first ROT_SAMPLES samples are achieved, so onReadingChanged2() will be used next time
    if (count>ROT_SAMPLES) {
        disconnect(m_rotation, SIGNAL(readingChanged()), this, SLOT(onReadingChanged()));
        connect(m_rotation, SIGNAL(readingChanged()), this, SLOT(onReadingChanged2()));
    }

    count++;
}

// Slot called when the circular list is filled
void Horizon::onReadingChanged2()
{
    // calculate an average of the last ROT_SAMPLES samples. This prevent horizon line to shake
    if (!m_reading) return;
    if (!m_rotation->isActive() || m_rotation->isBusy()) return;

    currRotX->value = m_reading->x();
    currRotY->value = m_reading->y();
    currRotZ->value = m_reading->z();

    for (int i=0; i<ROT_SAMPLES; i++, currRotX=currRotX->next, currRotY=currRotY->next, currRotZ=currRotZ->next) {
        m_average_rotX += currRotX->value;
        m_average_rotY += currRotY->value;
        m_average_rotZ += currRotZ->value;
    }

    m_average_rotX /= ROT_SAMPLES;
    m_average_rotY /= ROT_SAMPLES;
    m_average_rotZ /= ROT_SAMPLES;

    currRotX=currRotX->next->next;
    currRotY=currRotY->next->next;
    currRotZ=currRotZ->next->next;

    update();

    emit averageRotXChanged();
    emit averageRotYChanged();
    emit averageRotZChanged();
}
